export function python_xyz_to_psr(text, transform_matrix = null) {
  const points_array = text
    .split('\n')
    .filter(function (x) {
      return x
    })
    .map(function (x) {
      return x.split(' ').map(function (x) {
        return parseFloat(x)
      })
    })

  let boxes = points_array
  if (transform_matrix) {
    boxes = points_array.map(function (ps) {
      for (let i = 0; i < 8; i++) {
        var p = transform_point(transform_matrix, ps[3 * i + 0], ps[3 * i + 1], ps[3 * i + 2])
        ps[i * 3 + 0] = p[0]
        ps[i * 3 + 1] = p[1]
        ps[i * 3 + 2] = p[2]
      }
      return ps
    })
  }

  const boxes_ann = boxes.map(xyz_to_psr)

  return boxes_ann
}

function transform_point(m, x, y, z) {
  var rx = x * m[0] + y * m[1] + z * m[2]
  var ry = x * m[3] + y * m[4] + z * m[5]
  var rz = x * m[6] + y * m[7] + z * m[8]

  return [rx, ry, rz]
}

/*
    input is coordinates of 8 vertices
    bottom-left-front, bottom-right-front, bottom-right-back, bottom-left-back
    top-left-front,    top-right-front,    top-right-back,    top-left-back

    this format is what SECOND/PointRcnn save their results.
*/
/*
      5_______6
     /|      /|
    4_|_____7 |
    | |     | |
    | 1_____|_2
    |/      |/
    0_______3
  */
export function xyz_to_psr(ann_input) {
  swapPointPosition(ann_input, 0, 2)
  swapPointPosition(ann_input, 1, 3)
  swapPointPosition(ann_input, 4, 6)
  swapPointPosition(ann_input, 5, 7)
  let ann = []
  if (ann_input.length == 24) ann = ann_input
  else
    for (let i = 0; i < ann_input.length; i++) {
      if ((i + 1) % 4 != 0) {
        ann.push(ann_input[i])
      }
    }

  const pos = { x: 0, y: 0, z: 0 }
  for (let i = 0; i < 8; i++) {
    pos.x += ann[i * 3]
    pos.y += ann[i * 3 + 1]
    pos.z += ann[i * 3 + 2]
  }
  pos.x /= 8
  pos.y /= 8
  pos.z /= 8

  const scale = {
    x: Math.sqrt((ann[0] - ann[3]) * (ann[0] - ann[3]) + (ann[1] - ann[4]) * (ann[1] - ann[4])),
    y: Math.sqrt((ann[0] - ann[9]) * (ann[0] - ann[9]) + (ann[1] - ann[10]) * (ann[1] - ann[10])),
    z: ann[14] - ann[2],
  }

  /*
        1. atan2(y,x), not x,y
        2. point order in xy plane
            0   1
            3   2
    */

  const angle = Math.atan2(ann[4] + ann[7] - 2 * pos.y, ann[3] + ann[6] - 2 * pos.x)

  return {
    position: pos,
    scale: scale,
    rotation: { x: 0, y: 0, z: angle },
  }
}

// box(position, scale, rotation) to box corner corrdinates.
// return 8 points, represented as (x,y,z,1)
// note the vertices order cannot be changed, draw-box-on-image assumes
// the first 4 vertex is the front plane, so it knows box direction.
export function psr_to_xyz(p, s, r) {
  /*
  var trans_matrix=[
      Math.cos(r.z), -Math.sin(r.z), 0, p.x,
      Math.sin(r.z), Math.cos(r.z),  0, p.y,
      0,             0,              1, p.z,
      0,             0,              0, 1,
  ];
  */
  var trans_matrix = euler_angle_to_rotate_matrix(r, p)

  var x = s.x / 2
  var y = s.y / 2
  var z = s.z / 2
  /*
  var local_coord = [
      -x, y, -z, 1,   x, y, -z, 1,  //front-left-bottom, front-right-bottom
      x, y, z, 1,    -x, y, z, 1,  //front-right-top,   front-left-top

      -x, -y, -z, 1,   x, -y, -z, 1,  
      x, -y, z, 1,   -x, -y, z, 1,        
      
  ];
  */

  var local_coord = [
    x,
    y,
    -z,
    1,
    x,
    -y,
    -z,
    1, //front-left-bottom, front-right-bottom
    x,
    -y,
    z,
    1,
    x,
    y,
    z,
    1, //front-right-top,   front-left-top

    -x,
    y,
    -z,
    1,
    -x,
    -y,
    -z,
    1, //rear-left-bottom, rear-right-bottom
    -x,
    -y,
    z,
    1,
    -x,
    y,
    z,
    1, //rear-right-top,   rear-left-top
  ]

  var world_coord = matmul(trans_matrix, local_coord, 4)
  var w = world_coord
  return w
}

export function swapPointPosition(array, i, j) {
  swap(array, i * 3, j * 3)
  swap(array, i * 3 + 1, j * 3 + 1)
  swap(array, i * 3 + 2, j * 3 + 2)
}

function swap(array, i, j) {
  let tmp = array[i]
  array[i] = array[j]
  array[j] = tmp
}

export function euler_angle_to_rotate_matrix(eu, tr) {
  var theta = [eu.x, eu.y, eu.z]
  // Calculate rotation about x axis
  var R_x = [1, 0, 0, 0, Math.cos(theta[0]), -Math.sin(theta[0]), 0, Math.sin(theta[0]), Math.cos(theta[0])]

  // Calculate rotation about y axis
  var R_y = [Math.cos(theta[1]), 0, Math.sin(theta[1]), 0, 1, 0, -Math.sin(theta[1]), 0, Math.cos(theta[1])]

  // Calculate rotation about z axis
  var R_z = [Math.cos(theta[2]), -Math.sin(theta[2]), 0, Math.sin(theta[2]), Math.cos(theta[2]), 0, 0, 0, 1]

  //console.log(R_x, R_y, R_z);

  // Combined rotation matrix
  //var R = matmul(matmul(R_z, R_y, 3), R_x,3);
  var R = matmul2(R_x, matmul2(R_y, R_z, 3), 3)

  return [
    mat(R, 3, 0, 0),
    mat(R, 3, 0, 1),
    mat(R, 3, 0, 2),
    tr.x,
    mat(R, 3, 1, 0),
    mat(R, 3, 1, 1),
    mat(R, 3, 1, 2),
    tr.y,
    mat(R, 3, 2, 0),
    mat(R, 3, 2, 1),
    mat(R, 3, 2, 2),
    tr.z,
    0,
    0,
    0,
    1,
  ]
}

function mat(m, s, x, y) {
  return m[x * s + y]
}

// vl is vector length
export function matmul(m, x, vl) {
  var ret = []
  var res_l = m.length / vl
  // vector index
  for (var vi = 0; vi < x.length / vl; vi++) {
    // row of matrix
    for (var r = 0; r < m.length / vl; r++) {
      ret[vi * res_l + r] = 0
      for (var i = 0; i < vl; i++) {
        ret[vi * res_l + r] += m[r * vl + i] * x[vi * vl + i]
      }
    }
  }

  return ret
}

// vl is vector length
export function matmul2(m, x, vl) {
  var ret = []
  var rows = m.length / vl
  var cols = x.length / vl
  for (var r = 0; r < rows; r++) {
    for (var c = 0; c < cols; c++) {
      ret[r * cols + c] = 0
      for (var i = 0; i < vl; i++) {
        ret[r * cols + c] += m[r * vl + i] * x[i * cols + c]
      }
    }
  }

  return ret
}

export function vector4to3(v) {
  var ret = []
  for (var i = 0; i < v.length; i++) {
    if ((i + 1) % 4 != 0) {
      ret.push(v[i])
    }
  }

  return ret
}

export function vector3_nomalize(m) {
  var ret = []
  for (var i = 0; i < m.length / 3; i++) {
    ret.push(m[i * 3 + 0] / m[i * 3 + 2])
    ret.push(m[i * 3 + 1] / m[i * 3 + 2])
  }

  return ret
}
